#include <Servo.h> 
#include <elapsedMillis.h>

enum {
	CMD_STOP,
	CMD_FORWARD,
	CMD_BACKWARD,
	CMD_TURN_LEFT,
	CMD_TURN_RIGHT,
	CMD_SWITCH_MODE,

	CMD_NUM,
	CMD_INVALID = -1
};

typedef enum {
	OPERATION_MODE_AUTO,
	OPERATION_MODE_MANUAL,

	OPERATION_MODE_NUM,
	OPERATION_MODE_INVALID = -1
} OperationMode;

OperationMode currentOperationMode = OPERATION_MODE_MANUAL;

int leftMotor1 = 10;
int leftMotor2 = 11;
int rightMotor1 = 12;
int rightMotor2 = 13;

char lastCmd = CMD_STOP;

elapsedMillis timeElapsed;
unsigned int interval = 150;

// Declare the Servo pin 
const int servoPin = 6;

// Ultrasonic Sensor
const int trigPin = 8;
const int echoPin = 9;

#define DEGREES_INITIAL 110
#define DEGREES_MAX 60
#define DEGREES_STEP DEGREES_MAX

#define DISTANCE_INFINITE 50
#define DISTANCE_MIN 40

enum {
	DISTANCE_IDX_LEFT,
	DISTANCE_IDX_CENTER,
	DISTANCE_IDX_RIGHT,

	DISTANCE_IDX_NUM
};

int distances[DISTANCE_IDX_NUM] = { DISTANCE_INFINITE, DISTANCE_INFINITE, DISTANCE_INFINITE };
int degreesToMeasureDistance[DISTANCE_IDX_NUM] = { DEGREES_INITIAL + DEGREES_MAX, DEGREES_INITIAL, DEGREES_INITIAL - DEGREES_MAX };

int direction = 1;
int current = DEGREES_INITIAL;

// Create a servo object 
Servo Servo1; 

unsigned long microsecondsToCentimeters(unsigned long microseconds)
{
	// The speed of sound is 340 m/s or 29 microseconds per centimeter.
	// The ping travels out and back, so to find the distance of the
	// object we take half of the distance travelled.
	return microseconds / 29 / 2;
}

unsigned long getDistance() {
	unsigned long distance, pulseDuration;

	// The sensor is triggered by a HIGH pulse of 10 or more microseconds.
	// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
	pinMode(trigPin, OUTPUT);
	digitalWrite(trigPin, LOW);
	delayMicroseconds(2);
	digitalWrite(trigPin, HIGH);
	delayMicroseconds(10);
	digitalWrite(trigPin, LOW);

	// Read the signal from the sensor: a HIGH pulse whose
	// duration is the time (in microseconds) from the sending
	// of the ping to the reception of its echo off of an object.
	pinMode(echoPin, INPUT);
	pulseDuration = pulseIn(echoPin, HIGH);

	distance = microsecondsToCentimeters(pulseDuration);

	if (distance >= DISTANCE_INFINITE) {
		distance = DISTANCE_INFINITE;
	}

	return distance;
}

void motorRun(int cmd)
{
	switch(cmd){
		case CMD_FORWARD:
			Serial.println("CMD_FORWARD");
			digitalWrite(leftMotor1, HIGH);
			digitalWrite(leftMotor2, LOW);
			digitalWrite(rightMotor1, HIGH);
			digitalWrite(rightMotor2, LOW);
			break;
		 case CMD_BACKWARD:
			Serial.println("CMD_BACKWARD");
			digitalWrite(leftMotor1, LOW);
			digitalWrite(leftMotor2, HIGH);
			digitalWrite(rightMotor1, LOW);
			digitalWrite(rightMotor2, HIGH);
			break;
		 case CMD_TURN_LEFT:
			Serial.println("TURN	LEFT");
			digitalWrite(leftMotor1, HIGH);
			digitalWrite(leftMotor2, LOW);
			digitalWrite(rightMotor1, LOW);
			digitalWrite(rightMotor2, HIGH);
			break;
		 case CMD_TURN_RIGHT:
			Serial.println("TURN	RIGHT");
			digitalWrite(leftMotor1, LOW);
			digitalWrite(leftMotor2, HIGH);
			digitalWrite(rightMotor1, HIGH);
			digitalWrite(rightMotor2, LOW);
			break;
		case CMD_SWITCH_MODE:
			break;
		 default:
			Serial.println("CMD_STOP");
			digitalWrite(leftMotor1, LOW);
			digitalWrite(leftMotor2, LOW);
			digitalWrite(rightMotor1, LOW);
			digitalWrite(rightMotor2, LOW);
	}

	lastCmd = cmd;
}

void setup() { 
	Serial.begin(9600);

	pinMode(leftMotor1, OUTPUT);
	pinMode(leftMotor2, OUTPUT);
	pinMode(rightMotor1, OUTPUT);
	pinMode(rightMotor2, OUTPUT);

	// We need to attach the servo to the used pin number 
	Servo1.attach(servoPin); 
	Servo1.write(DEGREES_INITIAL); 
	delay(2000);
}

void autoModeLoop() {
	if (current >= (DEGREES_INITIAL + DEGREES_MAX) || current <= (DEGREES_INITIAL - DEGREES_MAX)) {
		direction *= -1;
	}

	current += DEGREES_STEP * direction;
	
	Servo1.write(current); 
	delay(200); 

	for (int i = 0; i < DISTANCE_IDX_NUM; i++) {
		if (current == degreesToMeasureDistance[i]) {
			distances[i] = getDistance();
			break;
		}
	}

	char tmp[32];
	sprintf(tmp, "%d, %d, %d", distances[0], distances[1], distances[2]);

	Serial.println(tmp);

	if (distances[DISTANCE_IDX_CENTER] < DISTANCE_MIN) {
		if (distances[DISTANCE_IDX_LEFT] < DISTANCE_MIN
				&& distances[DISTANCE_IDX_RIGHT] < DISTANCE_MIN) {
			motorRun(CMD_BACKWARD);
			delay(150);
			motorRun(CMD_TURN_RIGHT);
			delay(100);
		} else if (distances[DISTANCE_IDX_RIGHT] < DISTANCE_MIN) {
			motorRun(CMD_TURN_LEFT);
			delay(100);
		} else {
			motorRun(CMD_TURN_RIGHT);
			delay(100);
		}
	} else {
		motorRun(CMD_FORWARD);
	}
}

void manualModeLoop() {
	if (lastCmd != CMD_STOP && timeElapsed > interval) 
	{
		motorRun(CMD_STOP);
		timeElapsed = 0;			 // reset the counter to 0 so the counting starts over...
	}
}

void loop() {
	char cmd = CMD_INVALID;

	switch (currentOperationMode) {
		case OPERATION_MODE_AUTO:
			autoModeLoop();
			break;
		default:
			manualModeLoop();
			break;
	}

	// usart read
	if (Serial.available() > 0)
	{
		cmd = Serial.read();
		Serial.print(cmd);
		cmd = cmd - '0';

		if (cmd == CMD_SWITCH_MODE) {
			if (currentOperationMode == OPERATION_MODE_AUTO) {
				currentOperationMode = OPERATION_MODE_MANUAL;
			} else {
				currentOperationMode = OPERATION_MODE_AUTO;
			}
		}

		motorRun(cmd);

		timeElapsed = 0;
	}
}
